/*
 * gcode.cpp
 *
 * Created: 31.7.2013 21:28:41
 *  Author: Ferda
 */ 
#include <string.h>

 #include "gcode.h"

// compute buffer length in bytes for binary buffer data presented by masks
 OPTIMIZE_O3 uint8_t get_binary_code_size(uint8_t maskl, uint8_t maskh)
 {
	 uint8_t len = 4; // 2 bytes for mask + 2 bytes for checkum
	 
	 // N size
	 if (HAS_N(maskl))
		len += 2;
		
	// M size
	if (HAS_M(maskl))
		len += 2;
	
	// G size
	if (HAS_G(maskl))
		len += 1;
		
	// X
	if (HAS_X(maskl))
		len += 4;
	
	// Y
	if (HAS_Y(maskl))
		len += 4;
	
	// Z
	if (HAS_Z(maskl))
		len += 4;
	
	// E
	if (HAS_E(maskl))
		len += 4;
	
	//============================
	
	// F size
	if (HAS_F(maskh))
		len += 4;
	
	// T size
	if (HAS_T(maskh))
		len += 1;
		
	// S size
	if (HAS_S(maskh))
		len += 4;
		
	// P size
	if (HAS_P(maskh))
		len += 4;
		
	// V2
	if (HAS_V2(maskh))
		len += 1;
		
	// internal cmd size
	if (HAS_INTERNAL(maskh))
		len += 1;
		
	// Text size
	if (HAS_TEXT(maskh))
		len += 16;
	
	//=====================
	
	return len;
 }

//========================================================

// read content of binary buffer data to binary gcode 
// param buffer - pointer to binary packed buffer
// param ptr - pointer to unpacked binary gcode
// return next address to buffer to next read
OPTIMIZE_O3 uint8_t* buffer_to_binary_code(uint8_t *buffer, gcode_t *ptr)
{
	ptr->maskl = *buffer++;
	ptr->maskh = *buffer++;
	
	if (HAS_N(ptr->maskl)) 
	{
		ptr->N = *((uint16_t*)(buffer));
		buffer += 2;
	}
	
	if (HAS_M(ptr->maskl)) 
	{
		ptr->M = *((uint16_t*)(buffer));
		buffer += 2;
	}
	
	if (HAS_G(ptr->maskl)) 
	{
		ptr->G = *((uint8_t*)(buffer));
		buffer += 1;
	}
	
	if (HAS_X(ptr->maskl)) 
	{
		ptr->X = *((float*)(buffer));
		buffer += 4;
	}
	
	if (HAS_Y(ptr->maskl)) 
	{
		ptr->Y = *((float*)(buffer));
		buffer += 4;
	}
	
	if (HAS_Z(ptr->maskl)) 
	{
		ptr->Z = *((float*)(buffer));
		buffer += 4;
	}
	
	if (HAS_E(ptr->maskl)) 
	{
		ptr->E = *((float*)(buffer));
		buffer += 4;
	}
	
	//===============================
	
	if (HAS_F(ptr->maskh)) 
	{
		ptr->F = *((float*)(buffer));
		buffer += 4;
	}
	
	if (HAS_T(ptr->maskh)) 
	{
		ptr->T = *buffer;
		buffer += 1;
	}
	
	if (HAS_S(ptr->maskh)) 
	{
		ptr->S = *((uint32_t*)(buffer));
		buffer += 4;
	}
	
	if (HAS_P(ptr->maskh)) 
	{
		ptr->P = *((uint32_t*)(buffer));
		buffer += 4;
	}
	
	if (HAS_V2(ptr->maskh)) 
	{
		ptr->v2mask = *buffer;
		buffer += 1;
	}
	
	if (HAS_INTERNAL(ptr->maskh)) 
	{
		ptr->intcmd = *buffer;
		buffer += 1;
	}
	
	if (HAS_TEXT(ptr->maskh)) 
	{
		memcpy(ptr->text, buffer, 16);
		buffer += 16;
	}
	
	ptr->checkum = *((uint16_t*)(buffer));
	buffer += 2;
	
	return buffer;
}

//========================================================

// store binary unpacked gcode to packed binary gcode buffer
// param ptr - pointer to packed gcode
// param buffer - pointer to buffer start
// return pointer behind last element in buffer
OPTIMIZE_O3 uint8_t* binary_code_to_buffer(gcode_t *ptr, uint8_t *buffer)
{
	uint8_t *aux = buffer;
	
	*buffer++ = ptr->maskl;
	*buffer++ = ptr->maskh;
	
	if (HAS_N(ptr->maskl)) 
	{
		*((uint16_t*)(buffer)) = ptr->N;
		buffer += 2;
	}
	
	if (HAS_M(ptr->maskl)) 
	{
		*((uint16_t*)(buffer)) = ptr->M;
		buffer += 2;
	}
	
	if (HAS_G(ptr->maskl)) 
	{
		*((uint8_t*)(buffer)) = ptr->G;
		buffer += 1;
	}
	
	if (HAS_X(ptr->maskl)) 
	{
		*((float*)(buffer)) = ptr->X;
		buffer += 4;
	}
	
	if (HAS_Y(ptr->maskl)) 
	{
		*((float*)(buffer)) = ptr->Y;
		buffer += 4;
	}
	
	if (HAS_Z(ptr->maskl)) 
	{
		*((float*)(buffer)) = ptr->Z;
		buffer += 4;
	}
	
	if (HAS_E(ptr->maskl)) 
	{
		*((float*)(buffer)) = ptr->E;
		buffer += 4;
	}
	
	//================================
	
	if (HAS_F(ptr->maskh)) 
	{
		*((float*)(buffer)) = ptr->F;
		buffer += 4;
	}
	
	if (HAS_T(ptr->maskh)) 
	{
		*((uint8_t*)(buffer)) = ptr->T;
		buffer += 1;
	}
	
	if (HAS_S(ptr->maskh)) 
	{
		*((uint32_t*)(buffer)) = ptr->S;
		buffer += 4;
	}
	
	if (HAS_P(ptr->maskh)) 
	{
		*((uint32_t*)(buffer)) = ptr->P;
		buffer += 4;
	}
	
	if (HAS_V2(ptr->maskh)) 
	{
		*buffer++ = ptr->v2mask;
	}
	
	if (HAS_INTERNAL(ptr->maskh)) 
	{
		*buffer++ = ptr->intcmd;
	}
	
	if (HAS_TEXT(ptr->maskh)) 
	{
		memcpy(buffer, ptr->text, 16);
		buffer += 16;
	}
	
	*((uint16_t*)(buffer)) = calc_checkum16(aux, buffer - aux);
	buffer += 2;
	
	return buffer;
}

//================================================================
// see http://en.wikipedia.org/wiki/Fletcher's_checksum
//================================================================
	


